#include "xict_common/utils/atan.h"

/// \brief Look-up table for fast atan calculation (float precision)
const float LUT[102] = {
    0,           0.0099996664, 0.019997334, 0.029991005, 0.039978687,
    0.049958397, 0.059928156,  0.069885999, 0.079829983, 0.089758173,
    0.099668652, 0.10955953,   0.11942893,  0.12927501,  0.13909595,
    0.14888994,  0.15865526,   0.16839015,  0.17809294,  0.18776195,
    0.19739556,  0.20699219,   0.21655031,  0.22606839,  0.23554498,
    0.24497867,  0.25436807,   0.26371184,  0.27300870,  0.28225741,
    0.29145679,  0.30060568,   0.30970293,  0.31874755,  0.32773849,
    0.33667481,  0.34555557,   0.35437992,  0.36314702,  0.37185606,
    0.38050637,  0.38909724,   0.39762798,  0.40609807,  0.41450688,
    0.42285392,  0.43113875,   0.43936089,  0.44751999,  0.45561564,
    0.46364760,  0.47161558,   0.47951928,  0.48735857,  0.49513325,
    0.50284320,  0.51048833,   0.51806855,  0.52558380,  0.53303409,
    0.54041952,  0.54774004,   0.55499572,  0.56218672,  0.56931317,
    0.57637525,  0.58337301,   0.59030676,  0.59717667,  0.60398299,
    0.61072594,  0.61740589,   0.62402308,  0.63057774,  0.63707036,
    0.64350110,  0.64987046,   0.65617871,  0.66242629,  0.66861355,
    0.67474097,  0.68080884,   0.68681765,  0.69276786,  0.69865984,
    0.70449406,  0.71027100,   0.71599114,  0.72165483,  0.72726268,
    0.73281509,  0.73831260,   0.74375558,  0.74914461,  0.75448018,
    0.75976276,  0.76499283,   0.77017093,  0.77529752,  0.78037310,
    0.78539819,  0.79037325};

/// \brief Look-up table for fast atan calculation (double precision)
const double LUT_d[102] = {0,
                           0.00999966668666524,
                           0.0199973339731505,
                           0.0299910048568779,
                           0.0399786871232900,
                           0.0499583957219428,
                           0.0599281551212079,
                           0.0698860016346425,
                           0.0798299857122373,
                           0.0897581741899505,
                           0.0996686524911620,
                           0.109559526773944,
                           0.119428926018338,
                           0.129275004048143,
                           0.139095941482071,
                           0.148889947609497,
                           0.158655262186401,
                           0.168390157147530,
                           0.178092938231198,
                           0.187761946513593,
                           0.197395559849881,
                           0.206992194219821,
                           0.216550304976089,
                           0.226068387993884,
                           0.235544980720863,
                           0.244978663126864,
                           0.254368058553266,
                           0.263711834462266,
                           0.273008703086711,
                           0.282257421981491,
                           0.291456794477867,
                           0.300605670042395,
                           0.309702944542456,
                           0.318747560420644,
                           0.327738506780556,
                           0.336674819386727,
                           0.345555580581712,
                           0.354379919123438,
                           0.363147009946176,
                           0.371856073848581,
                           0.380506377112365,
                           0.389097231055278,
                           0.397627991522129,
                           0.406098058317616,
                           0.414506874584786,
                           0.422853926132941,
                           0.431138740718782,
                           0.439360887284591,
                           0.447519975157170,
                           0.455615653211225,
                           0.463647609000806,
                           0.471615567862328,
                           0.479519291992596,
                           0.487358579505190,
                           0.495133263468404,
                           0.502843210927861,
                           0.510488321916776,
                           0.518068528456721,
                           0.525583793551610,
                           0.533034110177490,
                           0.540419500270584,
                           0.547740013715902,
                           0.554995727338587,
                           0.562186743900029,
                           0.569313191100662,
                           0.576375220591184,
                           0.583373006993856,
                           0.590306746935372,
                           0.597176658092678,
                           0.603982978252998,
                           0.610725964389209,
                           0.617405891751573,
                           0.624023052976757,
                           0.630577757214935,
                           0.637070329275684,
                           0.643501108793284,
                           0.649870449411948,
                           0.656178717991395,
                           0.662426293833151,
                           0.668613567927821,
                           0.674740942223553,
                           0.680808828915828,
                           0.686817649758645,
                           0.692767835397122,
                           0.698659824721463,
                           0.704494064242218,
                           0.710271007486686,
                           0.715991114416300,
                           0.721654850864761,
                           0.727262687996690,
                           0.732815101786507,
                           0.738312572517228,
                           0.743755584298860,
                           0.749144624606017,
                           0.754480183834406,
                           0.759762754875771,
                           0.764992832710910,
                           0.770170914020331,
                           0.775297496812126,
                           0.780373080066636,
                           0.785398163397448,
                           0.790373246728302};

float atan_fast(float x) {
    if (x > 0) {
        if (x <= 1) {
            int index = round(x * 100);
            return LUT[index];
        } else {
            float re_x = 1 / x;
            int index  = round(re_x * 100);
            return (M_PI_2 - LUT[index]);
        }
    } else {
        if (x >= -1) {
            float abs_x = -x;
            int index   = round(abs_x * 100);
            return -(LUT[index]);
        } else {
            float re_x = 1 / (-x);
            int index  = round(re_x * 100);
            return (LUT[index] - M_PI_2);
        }
    }
}

float atan_single(float x) {
    if (x >= 0) {
        if (x <= 1) {
            int index = round(x * 100);
            return (LUT[index] +
                    (x * 100 - index) * (LUT[index + 1] - LUT[index]));
        } else {
            float re_x = 1 / x;
            int index  = round(re_x * 100);
            return (M_PI_2 - (LUT[index] + (re_x * 100 - index) *
                                               (LUT[index + 1] - LUT[index])));
            // No recursive is better here
        }
    } else {
        if (x >= -1) {
            float abs_x = -x;
            int index   = round(abs_x * 100);
            return -(LUT[index] +
                     (abs_x * 100 - index) * (LUT[index + 1] - LUT[index]));
        } else {
            float re_x = 1 / (-x);
            int index  = round(re_x * 100);
            return (LUT[index] +
                    (re_x * 100 - index) * (LUT[index + 1] - LUT[index])) -
                   M_PI_2;
        }
    }
}

double atan_double(double x) {
    if (x >= 0) {
        if (x <= 1) {
            int index = round(x * 100);
            return (LUT_d[index] +
                    (x * 100 - index) * (LUT_d[index + 1] - LUT_d[index]));
        } else {
            double re_x = 1 / x;
            int index   = round(re_x * 100);
            return (M_PI_2 -
                    (LUT_d[index] +
                     (re_x * 100 - index) * (LUT_d[index + 1] - LUT_d[index])));
            // No recursive is better here
        }
    } else {
        if (x >= -1) {
            double abs_x = -x;
            int index    = round(abs_x * 100);
            return -(LUT_d[index] +
                     (abs_x * 100 - index) * (LUT_d[index + 1] - LUT_d[index]));
        } else {
            double re_x = 1 / (-x);
            int index   = round(re_x * 100);
            return (LUT_d[index] +
                    (re_x * 100 - index) * (LUT_d[index + 1] - LUT_d[index])) -
                   M_PI_2;
        }
    }
}

float smoothstep(float x) {
    x = std::fmax(0.0f, std::fmin(1.0f, x));
    return x * x * (3 - 2 * x);
}

std::pair<float, float> polyfit(const std::vector<float>& x,
                                const std::vector<float>& y) {
    float sum_x = 0, sum_y = 0, sum_xy = 0, sum_xx = 0;
    int n = x.size();

    // Calculate sums for the linear regression formula
    for (int i = 0; i < n; ++i) {
        sum_x += x[i];
        sum_y += y[i];
        sum_xy += x[i] * y[i];
        sum_xx += x[i] * x[i];
    }

    // Calculate slope and intercept
    float slope = (n * sum_xy - sum_x * sum_y) / (n * sum_xx - sum_x * sum_x);
    float intercept = (sum_y - slope * sum_x) / n;

    return {slope, intercept};
}

float polyval(const std::pair<float, float>& p, float x) {
    return p.first * x + p.second;
}
